/*
 * Calculate.c
 *
 *  Created on: 2024年12月28日
 *      Author: 28186
 */

/*******************************最小二乘法***************************************/
#include "zf_common_headfile.h"
//计算斜率
/**
* 函数功能：      最小二乘法计算斜率
* 特殊说明：      无
* 形  参：        uint8 begin                输入起点
*                 uint8 end                  输入终点
*                 uint8 *border              输入需要计算斜率的一维边线数组
* 示例：          Slope_Calculate(start, end, border);
* 返回值：        Result    计算出的斜率
*/
float Slope_Calculate(uint8 begin, uint8 end, uint8 *border)    //注：begin 必须小于 end,一般 begin 位于图像上方， end 位于图像下方
{
    float X_Sum = 0, Y_Sum = 0, XY_Sum = 0, X2_Sum = 0;
    int16 i = 0;
    float Result = 0;
    static float Result_Last;

    for(i = begin; i < end ; i++)
    {
        X_Sum += (float)i;
        Y_Sum += (float)border[i];
        XY_Sum += (float)i * (border[i]);
        X2_Sum += (float)i * i;
    }

    if((end - begin) * X2_Sum - X_Sum * X_Sum)      //防止为0的情况出现
    {
        Result = ((float)(end - begin) * XY_Sum - X_Sum * Y_Sum) / ((float)(end - begin) * X2_Sum - X_Sum * X_Sum);
        Result_Last = Result;
    }
    else
    {
        Result = Result_Last;
    }
    return Result;
}


/**
* 函数功能：      计算斜率截距
* 特殊说明：      调用最小二乘法计算斜率
* 形  参：        uint8 start                输入起点
*                 uint8 end                  输入终点
*                 uint8 *border              输入需要计算斜率的一维边线数组
*                 float *slope_rate          存储斜率的变量地址
*                 float *intercept           存储截距的变量地址
* 示例：          Calculate_Slope_Intercept(start, end, L_Border, &L_Straightaway_Lope_Rate_C, &L_Intercept);
* 返回值：        无
*/
void Calculate_Slope_Intercept(uint8 start, uint8 end, uint8 *border, float *slope_rate, float *intercept)
{
    uint16 i, Num = 0;
    uint16 X_Sum = 0, Y_Sum = 0;
    float Y_Average = 0, X_Average = 0;

    for(i = start; i < end; i++)
    {
        X_Sum += i;
        Y_Sum += border[i];
        Num ++;
    }

    if(Num)
    {
        X_Average = (float)(X_Sum / Num);
        Y_Average = (float)(Y_Sum / Num);
    }

    *slope_rate = Slope_Calculate(start, end, border);
    *intercept = (float)(Y_Average - (*slope_rate) * X_Average);
}
// int Scan_Mun3[70]={
//     1,1,1,1,1,1,1,1,1,1,//0~9
//     1,1,1,1,1,1,1,1,1,1,//10~19
//     1,1,1,1,1,1,1,1,1,1,//20~29
//     1,1,1,1,1,1,1,1,1,1,//30~39
//     1,1,1,1,1,1,1,1,1,1,//40~49
//     1,1,1,1,1,1,1,1,1,1,//50~59
//     1,1,1,1,1,1,1,1,1,1,//60~69
//                  };//误差权重
// float Last_Curvature_Value = 0;     //防止
// /**
// * 函数功能：      计算中线曲率，作为控制速度的一个数据
// * 特殊说明：      无
// * 形  参：        uint8 *line      中线
// *                 uint8 start      爬线相遇点的Y值，或最大有效行
// *                 uint8 end        爬线起始行（或图像最底端）
// *
// * 示例：          Calculate_Curvature_2(C_Line, Y_Meet, L_Start_Point[1]);
// * 返回值：        Last_Curvature_Value      所得曲率
// */
// float Calculate_Curvature_2(uint8 *line, uint8 start, uint8 end)
// {
//     int16 Sum_Difference_Value = 0;     //用于X坐标差值的累积
//     int16 Sum_Weight = 0;               //用于权值相加
//     uint8 i = 0;
 
//     // // uint8 Base_Curvature_Weight[60] = {0, 0, 1, 0, 1, 0, 1, 0, 1, 0,
//     // //                                    1, 0, 1, 0, 1, 0, 1, 0, 1, 0,
//     // //                                    1, 0, 1, 0, 1, 0, 1, 0, 1, 0,
//     // //                                    1, 0, 1, 0, 1, 0, 1, 0, 1, 0,
//     // //                                    1, 0, 1, 0, 1, 0, 1, 0, 1, 0,
//     // //                                    1, 0, 1, 0, 1, 0, 1, 0, 1, 0};       //固定权值，0,1,0,1间隔加权
//     // uint8 Trends_Curvature_Weight[19] = {4, 5, 5, 6, 6, 7, 7, 8, 8, 8, 8, 8, 7, 7, 6, 6, 5, 5, 4};      //动态权值，高权值放中间
 
//     // uint8 Start_Line = (uint8)((float)(hightest - 2) / 18.0f * 16.0f + 24.0f);    //动态权替换固定权起始行，我图像为60行，最低从24行向上替换，最高为40行向上替换，Y_Meet最大值为20
 
//     // //将动态权值赋值给固定权值数组
//     // for(i = 0; i < 19; i ++)
//     // {
//     //     Scan_Mun2[Start_Line - i] = Trends_Curvature_Weight[i];
//     // }
//     for(i = start; i < end - 1; i++)    //求差值和并求出权值
//     {
//         Sum_Difference_Value += (int16)(abs((int)line[i] - (int)line[i + 1]) * (int)Scan_Mun2[i]);
//         Sum_Weight += (int16)Scan_Mun2[i];
//     }
 
//     if(Sum_Weight != 0)     //防止权值为0，但好像不会出现，当时不知道为啥要写这个
//     {
//         Last_Curvature_Value = (float)Sum_Difference_Value / (float)Sum_Weight;     //求加权平均值，归一化放在了另一个函数里
//         return Last_Curvature_Value;
//     }
//     else
//     {
//         return Last_Curvature_Value;
//     }
// }

// float Speed_Value[7] = {0.0f, 0.166f, 0.333f, 0.5f, 0.666f, 0.833f, 1.0f};  //速度采用了归一化，将1.0均分为六段
 
// float Speed_Vague_Array[4][4] = { {0, 1, 2, 3},
//                                   {1, 2, 3, 4},
//                                   {3, 4, 5, 6},
//                                   {5, 6, 6, 6}};    //映射数组
 
// int16 Y_Meet_Count = 0;     //累积满足相遇点条件的次数，比如由弯道进入直道时，车身未转正时Y_Meet已经趋近于2了，此时加速会导致车轨迹不稳
//                             //那我们就进行一定的缓冲，当Y_Meet满足我们设定的条件一定次数后，再去执行相应的加速程序，可确保车身转正再加速
// /**
// * 函数功能：      使用相遇点和曲率映射速度，参照模糊PID思想
// * 特殊说明：      无
// * 形  参：        float sensitivity                //曲率
// *                 float speed_min_proportion       //速度最小比率（设定最小速度与最大速度的比值），为0~1之间
// *                 float Speed_max_proportion       //速度最大比率，通常为1
// *                 uint8 y_meet                     //爬线相遇点的Y坐标
// *                 uint8 min_y_meet                 //爬线相遇点的最小值，看得越远值越小
// *                 uint8 max_y_meet                 //爬线相遇点的最大值，看得越远值越大
// *
// * 示例：          Calculate_Curvature_2(C_Line, Y_Meet, L_Start_Point[1]);
// * 返回值：        Last_Curvature_Value      所得曲率
// */
// float Speed_Mapping(float sensitivity, float speed_min_proportion, float Speed_max_proportion, uint8 y_meet, uint8 min_y_meet, uint8 max_y_meet)
// {
//     // uint16 i = 0;
//     float VH = 0, VE = 0;
//     float X2Y = 0;
//     float X1Y = 0;
//     float Y2X = 0;
//     float Y1X = 0;
//     uint8 View_Effictive_Line = max_y_meet - min_y_meet;
//     uint8 Y_Meet=hightest;
//     int Max_Speed=Speed_Target;
//     int Min_Speed=Speed_Target*2/3;
//     // //弯道、环岛、十字时设定最大速度为35（这是我们组控制位给的一个值，换成自己组的）
//     // if(Element_State == L_Turn ||  Element_State == R_Turn || Element_State == L_Circle || Element_State == R_Circle || Element_State == Cross)
//     // {
//     //     Y_Meet_Count = 0;
//     //     Max_Speed = 35;
//     // }
//     //下面就是所说的缓冲代码，弯道到直道切换时，爬线相遇点Y趋近于2，当满足一千次以后（计算一千张图像），将最大速度设定为40
//     //另一方面更重要的是为了防止误判，导致乱加减速
//     //冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！
//     //冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！
//     //冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！
//     if(Y_Meet <= 4 && Max_Speed == 35)
//     {
//         Y_Meet_Count ++;
//         if(Y_Meet_Count >= 1000)
//         {
//             Y_Meet_Count = 0;
//             Max_Speed = 40;
//         }
//     }
//     //一次不满足就重新来，比较严格，可以不用或者判定放缓一些
//     else if(Y_Meet != 2 && Max_Speed == 35)
//     {
//         Max_Speed = 35;
//         Y_Meet_Count = 0;
//     }
//     //直道到弯道的缓冲（作用于弯道在图像最远端，还未识别到弯道时），这个缓冲时间短一些，减速快些，这两个计数值根据自己情况调整
//     else if(Y_Meet != 2 && Max_Speed == 40)
//     {
//         Y_Meet_Count --;
//         if(Y_Meet_Count <= -350)
//         {
//             Y_Meet_Count = 0;
//             Max_Speed = 35;
//         }
//     }
 
//     // //获取左右两侧边线落在边框上的个数
//     // Get_L_Border_Point_Num();
//     // Get_R_Border_Point_Num();
 
//     //当识别出元素是直道，爬线相遇点位于顶端，左右两侧几乎没有落在边框上的点，并且中线曲率很小时，直接返回最大速度开冲！
//     //此处判定十分严格，不会误判，不满足时才执行下方速度拟合部分代码
//     if(Y_Meet <= 3 && L_Border_Point_Num <= 2 && R_Border_Point_Num <= 2 && sensitivity <= 0.2f)
//     {
//         return Max_Speed;
//     }
//     //下面使用曲率和爬线相遇点Y坐标拟合速度
//     //与模糊PID原理相同，不过多注释
//     VH = ((float)(Y_Meet - 2) * 3.0f / (float)View_Effictive_Line);
//     VE = sensitivity * 3.0f;
 
//     int8 VH1 = (int8)VH;
//     if (VH1 > VH)
//     {
//       VH--;
//     }
//     int8 VH2 = VH1 + 1;
 
//     int8 VE1 = (int8)VE;
//     if (VE1 > VE)
//     {
//       VE1--;
//     }
//     int8 VE2 = VE1 + 1;
 
//     if (VH1 > 3)
//     {
//       VH1 = 3;
//     }
 
//     if (VH2 > 3)
//     {
//       VH2 = 3;
//     }
 
//     if (VE1 > 3)
//     {
//       VE1 = 3;
//     }
 
//     if (VE2 > 3)
//     {
//       VE2 = 3;
//     }
 
//     X2Y = (Speed_Vague_Array[VH1][VE2] - Speed_Vague_Array[VH1][VE1]) * (VE - VE1) + Speed_Vague_Array[VH1][VE1];
 
//     X1Y = (Speed_Vague_Array[VH2][VE2] - Speed_Vague_Array[VH2][VE1]) * (VE - VE1) + Speed_Vague_Array[VH2][VE1];
 
//     Y2X = (Speed_Vague_Array[VH2][VE1] - Speed_Vague_Array[VH1][VE1]) * (VH - VH1) + Speed_Vague_Array[VH1][VE1];
 
//     Y1X = (Speed_Vague_Array[VH2][VE2] - Speed_Vague_Array[VH1][VE2]) * (VH - VH1) + Speed_Vague_Array[VH1][VE2];
 
//     float Speed_approximation = (X2Y + X1Y + Y2X + Y1X) / 4.0;
 
//     int8 Speed_1 = (int8)Speed_approximation;
//     if (Speed_1 > Speed_approximation)
//     {
//         Speed_1--;
//     }
//     int8 Speed_2 = Speed_1 + 1;
 
//     return (1.0f - ((Speed_Value[Speed_2] - Speed_Value[Speed_1]) * (Speed_approximation - Speed_1) + Speed_Value[Speed_1])) * (float)(Max_Speed - Min_Speed) + (float)Min_Speed;   //返回拟合好的速度
// }
